#ifndef FUNCLOG_H
#define FUNCLOG_H



#include "types.h"
#include "driverSD_SPI_DMA_ISR_TASK.h"

void logTick();

#define LOG_BUFFERS				2		// total number of buffers
#define LOG_STORAGE_SIZE		BLOCK_SIZE	// bytes per buffer

#define LOG_STATE_IN_USE		0
#define LOG_STATE_FILLED		1
#define LOG_STATE_WRITING		2
#define LOG_STATE_EMPTY			3



enum {
    LOG_ID_TIME = 0,
    LOG_ID_TIME_CHAR = 1,
    LOG_ID_TIME_INC = 2,
    LOG_ID_BATTERY = 16,
    LOG_ID_IMU_MODE,
    LOG_ID_ACC_RAW,
    LOG_ID_ACC_FILTERED,
    LOG_ID_GYRO_RAW,
    LOG_ID_GYRO_FILTERED,
    LOG_ID_MAGN_RAW,
    LOG_ID_MAGN_FILTERED,
    LOG_ID_BARO_RAW,
    LOG_ID_BARO_FILTERED,
    LOG_ID_IMU_HEADING,
    LOG_ID_RC_INPUT,
    LOG_ID_MOTOR_1,
    LOG_ID_MOTOR_2,
    LOG_ID_MOTOR_3,
    LOG_ID_MOTOR_4,
    LOG_ID_MOTOR_5,
    LOG_ID_MOTOR_6,
    LOG_ID_MOTOR_DEBUG,
    LOG_ID_MOTOR_BODE,
    LOG_ID_POWER_CURRENT,
    LOG_ID_PID_0,
    LOG_ID_PID_1,
    LOG_ID_PID_2,
    LOG_ID_PID_3,
    LOG_ID_PID_4,
    LOG_ID_PID_5,
    LOG_ID_PID_6,
    LOG_ID_PID_7,
    LOG_ID_PID_8,
    LOG_ID_PID_9,
    LOG_ID_PID_10,
    LOG_ID_PID_11,
    LOG_ID_PID_12,
    LOG_ID_PID_13,
    LOG_ID_PID_14,
    LOG_ID_PID_15,
    LOG_ID_MAX_VALUE
} LOG_IDS;


struct LogBuffer_t
{
	BYTE			state;
	UINT32          packet;
	BYTE			writeBuffer[LOG_STORAGE_SIZE];
};

struct LogDefinition_t
{
    char * name;
    unsigned char byteLength;
    unsigned char numValues;
    unsigned char isSigned;
    unsigned char intBits;
    unsigned char floatBits;
};

void btLogSwitchBuffer();
void logSwitchBuffer();

#define BT_LOG_BUFFER_SIZE          4096
#define BT_LOG_BUFFER_THRESHOLD     (BT_LOG_BUFFER_SIZE - 256)

//*(logPointer++) = *((UINT32*)(&value)); /*((value).x << 16) | ((value).y); \*/ \
//    *(logPointer++) = ((value).z); \
//    TickType_t tempTime = time; \

#define logStore(time, id, value) { \
    extern struct LogBuffer_t logBuffer[]; \
    extern BYTE * logPointer; \
    extern BYTE * logPointerEnd; \
    extern BYTE btLogBufferOverflow; \
    extern BYTE * btLogBufferPos; \
    extern BYTE * btLogBufferPosEnd; \
    extern struct LogDefinition_t logDefinition[]; \
    if (STATE.sdLogging.loggingEnabled) { \
        sdLogDataCheckTimestamp(time); \
        if (logPointer + logDefinition[id].byteLength + 1 >= logPointerEnd) logSwitchBuffer(); \
        *(logPointer++) = id; \
        memcpy(logPointer,&(value),logDefinition[id].byteLength); \
        logPointer += logDefinition[id].byteLength; \
    } \
    if (STATE.btLogging.varEnabled[id] != 0) { \
        if (STATE.btLogging.varLastSent[id] <= time - STATE.btLogging.varEnabled[id]) { \
            STATE.btLogging.varLastSent[id] = time; \
            btLogDataCheckTimestamp(time); \
            if (btLogBufferOverflow || btLogBufferPos + 1 + logDefinition[id].byteLength >= btLogBufferPosEnd) \
            { \
                btLogBufferOverflow = TRUE; \
            } \
            else \
            { \
                *(btLogBufferPos++) = id; \
                memcpy(btLogBufferPos, &(value), logDefinition[id].byteLength); \
                btLogBufferPos += logDefinition[id].byteLength; \
            } \
        } \
    } \
}

#endif
